%% 文献《三维多站侧向交叉定位算法及精度分析 李洪梅》仿真案例复现 %%
clc
clear

% 三个测量站按三角形方式布站
% sigma_theta = 1*pi/180;% 方位角测量误差
% sigma_fi = 1*pi/180;% 高低角测量误差
sigma_theta = 2*pi/180;% 方位角测量误差
sigma_fi = 2*pi/180;% 高低角测量误差
sigma_s = 0.01; % 站址误差

S1 = [20 0 0];% 监测站1坐标
x1 = S1(1);y1 = S1(2);z1 = S1(3);
S2 = [-20 0 0];% 监测站2坐标
x2 = S2(1);y2 = S2(2);z2 = S2(3);
S3 = [0 -30 0];% 监测站3坐标
x3 = S3(1);y3 = S3(2);z3 = S3(3);

% 观测误差的协方差矩阵
dV_dVT = diag([sigma_theta^2 sigma_fi^2 ...
               sigma_theta^2 sigma_fi^2 ...
               sigma_theta^2 sigma_fi^2]);
K = 100;% 正方形边长
[X,Y] = meshgrid(-K:1:K);
X(101,:) = [];
Y(101,:) = [];
X(71,:) = [];
Y(71,:) = [];
z = 10;
GDOP = zeros(2*K-1,2*K+1);
for i = 1:2*K-1
    for j = 1:2*K+1
        x = X(i,j);
        y = Y(i,j);
        % 计算单个gdop
        % 站1 
        theta1 = atan((x-x1)/(y-y1));
        r1 = sqrt((x-x1)^2+(y-y1)^2);
        fi1 = atan((z-z1)/r1);
        % 站2
        theta2 = atan((x-x2)/(y-y2));
        r2 = sqrt((x-x2)^2+(y-y2)^2);
        fi2 = atan((z-z2)/r2);
        % 站3
        theta3 = atan((x-x3)/(y-y3));
        r3 = sqrt((x-x3)^2+(y-y3)^2);
        fi3 = atan((z-z3)/r3);
        % 系数矩阵
        F = [(cos(theta1))^2/(y-y1) sin(theta1)*cos(theta1)/(y1-y) 0;
            (x1-x)*cos(fi1)*sin(fi1)/(r1^2) (y1-y)*cos(fi1)*sin(fi1)/(r1^2) (cos(fi1))^2/r1
            (cos(theta2))^2/(y-y2) sin(theta2)*cos(theta2)/(y2-y) 0;
            (x2-x)*cos(fi2)*sin(fi2)/(r2^2) (y2-y)*cos(fi2)*sin(fi2)/(r2^2) (cos(fi2))^2/r2
            (cos(theta3))^2/(y-y3) sin(theta3)*cos(theta3)/(y3-y) 0;
            (x3-x)*cos(fi3)*sin(fi3)/(r3^2) (y3-y)*cos(fi3)*sin(fi3)/(r3^2) (cos(fi3))^2/r3];
        h1 = [(cos(theta1))^2/(y1-y) sin(theta1)*cos(theta1)/(y-y1) 0;
            (x-x1)*cos(fi1)*sin(fi1)/(r1^2) (y-y1)*cos(fi1)*sin(fi1)/(r1^2) -(cos(fi1))^2/r1];
        h2 = [(cos(theta2))^2/(y2-y) sin(theta2)*cos(theta2)/(y-y2) 0;
            (x-x2)*cos(fi2)*sin(fi2)/(r2^2) (y-y2)*cos(fi2)*sin(fi2)/(r2^2) -(cos(fi2))^2/r2];
        h3 = [(cos(theta3))^2/(y3-y) sin(theta3)*cos(theta3)/(y-y3) 0;
            (x-x3)*cos(fi3)*sin(fi3)/(r3^2) (y-y3)*cos(fi3)*sin(fi3)/(r3^2) -(cos(fi3))^2/r3];
        % 布站误差的协方差矩阵
        dXs_dXsT = sigma_s^2.*blkdiag(h1,h2,h3)*blkdiag(h1,h2,h3)';
        C = (F'*F)\F';
        % 定位误差的协方差矩阵
        P = C*(dV_dVT+dXs_dXsT)*C';
        GDOP(i,j) = sqrt(trace(P));
    end
end
figure
surf(X,Y,GDOP)
xlabel('x(km)');ylabel('y(km)');zlabel('gdop');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
   
figure
% contour(X,Y,GDOP,[0.3 0.5 1 2 3 4 5 8 10],'ShowText','on');hold;
contour(X,Y,GDOP,[0.3 0.5 1 2 4 5 8 10 20 50],'ShowText','on');hold;
scatter([20 -20 0],[0 0 -30],'filled','red');
xlabel('x(km)');ylabel('y(km)');
% title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);

figure
[row,column] = find(abs(GDOP-min(min(GDOP))) <= 0.0001);
contour(X,Y,GDOP,[0.48 0.49 0.5 1 2 4 5 8 10 20 50],'ShowText','on');hold;
scatter([20 -20 0],[0 0 -30],'filled','red');hold;
scatter(X(row,column),Y(row,column),'filled','blue');
xlabel('x(km)');ylabel('y(km)');
% title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);